/*
 * InputNode.cpp
 *
 *  Created on: 2012-05-27
 *      Author: mathieu
 */

#include <ros/ros.h>
#include <utilite/AudioFrameFreqSqrdMagn.h>
#include "semol/Sensorimotor.h"
#include <opencv2/core/core.hpp>
#include "MsgConversion.h"

class AudioInput
{
public:
	AudioInput(ros::NodeHandle n) :
		n_(n)
	{
		audio_sub = n_.subscribe("audioFrameFreqSqrdMagn", 1, &AudioInput::imageAudioCallback, this);
		sensorimotor_pub_ = n_.advertise<semol::Sensorimotor>("sensorimotor",1);
	}

	void imageAudioCallback(const utilite::AudioFrameFreqSqrdMagnConstPtr & audio)
	{
		if(!audio->data.size())
		{
			ROS_ERROR("Audio is empty...");
			return;
		}

		// Create a sensorimotor msg
		semol::SensorimotorPtr sm(new semol::Sensorimotor());
		sm->header.stamp = ros::Time::now();
		sm->sensors.resize(1);

		//audio
		cv::Mat dataMat(audio->nChannels, audio->frameLength, CV_32F);
		memcpy(dataMat.data, audio->data.data(), audio->data.size()*sizeof(float));
		sm->sensors[0].type = semol::Sensor::kTypeAudioFreqSqrdMagn;
		fromCvMatToCvMatMsg(sm->sensors[0].matrix, dataMat, false);

		sensorimotor_pub_.publish(sm);
	}

private:
	ros::NodeHandle n_;

	//inputs
	ros::Subscriber audio_sub;

	ros::Publisher sensorimotor_pub_;
};


int main(int argc, char** argv)
{
	ros::init(argc, argv, "audio_input");
	ros::NodeHandle n;
	AudioInput iai(n);

	ros::spin();

	return 0;
}

